#ifndef QUATERNION
#define QUATERNION

#include "matrix3.h"
#include "matrix4.h"

namespace sleek
{
    namespace core
    {
        namespace math
        {
            template <class T>
            class quaternion
            {
                public:
                    inline quaternion() : a(0), b(0), c(0), d(0) {}
                    inline quaternion(T _a,T _b,T _c,T _d) : a(_a), b(_b), c(_c), d(_d) { }
                    inline quaternion(const vector3d<T> &axis, T angle)
                    {
                        T s = sin(angle/2);
                        vector3d<T> axisn = axis.normalise();
                            a = cos(angle/2);
                            b = axisn.x*s;
                            c = axisn.y*s;
                            d = axisn.z*s;
                    }
                    inline quaternion<T> operator *(const quaternion<T> &q2) const
                    {
                        quaternion<T> result;
                            result.a = a*q2.a - b*q2.b - c*q2.c - d*q2.d;
                            result.b = a*q2.b + b*q2.a + c*q2.d - d*q2.c;
                            result.c = a*q2.c - b*q2.d + c*q2.a + d*q2.b;
                            result.d = a*q2.d + b*q2.c - c*q2.b + d*q2.a;
                        return result;
                    }
                    inline quaternion<T> operator *(T s) const
                    {
                        quaternion<T> result;
                            result.a = a*s;
                            result.b = b*s;
                            result.c = c*s;
                            result.d = d*s;
                        return result;
                    }
                    inline T Length(const quaternion<T> &q)
                    {
                        return sqrt( q.a*q.a + q.b*q.b + q.c*q.c + q.d*q.d );
                    }
                    inline void Normalize(quaternion<T> &q)
                    {
                        T il=1/Length(q);
                        q*=il;
                    }
                    inline quaternion<T> inverse(const quaternion<T> &q)
                    {
                        return quaternion<T>(-q.a , q.b , q.c , q.d);
                    };
                    inline quaternion<T>& operator *=(T s)
                    {
                        a *= s; b *= s; c *= s; d *= s;
                        return *this;
                    }
                public:
                    T a,b,c,d;
            };
            template <class T> matrix3<T> QuaternionToMatrix(const quaternion<T> &q)
            {
                    T sq[4]={q.a*q.a , q.b*q.b , q.c*q.c , q.d*q.d};
                    matrix3<T> result(
                        sq[0]+sq[1]-sq[2]-sq[3],        2*(q.b*q.c-q.a*q.d),            2*(q.a*q.c+q.b*q.d),
                        2*(q.a*q.d+q.b*q.c),            sq[0]+sq[2]-sq[1]-sq[3],        2*(q.c*q.d-q.a*q.b),
                        2*(q.b*q.d-q.a*q.c),            2*(q.a*q.b+q.c*q.d),            sq[0]+sq[3]-sq[1]-sq[2]);
                    return result;
            };
            typedef quaternion<f32> quaternionf;
            typedef quaternion<int> quaternioni;
            typedef quaternion<u32> quaternionu;
            typedef quaternion<s32> quaternions;
        }
    }
}

#endif
